/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Implementation of Position interface Server.
 * \author Antti Maula <antti.maula@tkk.fi>
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */

#include "PositionServer.hpp"
#include "binbag.h"

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
using namespace MaCI::Position;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CPositionServer::CPositionServer(MaCICtrl::CMaCICtrlServer *aMaCICtrlServerPtr, 
                                 const int aInterfaceMinor,
                                 const std::string &aDescription)
  : CMaCI(aMaCICtrlServerPtr, GIMI_PROTOCOL_MACI_POSITION, aInterfaceMinor),
    CSync(1,1),
    iServerActive(false),
    iServiceDescription(aDescription)
{
  SetInterfaceSubscriberSyncLimits(5, 1000);
}
//*****************************************************************************

CPositionServer::~CPositionServer()
{
  DoClose();
}
//*****************************************************************************

MaCI::EMaCIError CPositionServer::DoOpen(void)
{
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);
  
  // Execute only if server is not active.
  if (!iServerActive) {
    // Register service.
    RegisterProvidedService("MaCI-PositionServer-Position; '" + iServiceDescription + "'");
    RegisterAcceptedService("MaCI-PositionServer-Cmd; '" + iServiceDescription + "'");
    SetActive(true);
    iServerActive = true;

    result = KMaCIOK;
    
    dPrint(4,"Succesfully Open():ed PositionServer instance %p.", this);
  }
    
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CPositionServer::DoClose(void)
{
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);

  // Execute block only if server is currently active.
  if (iServerActive) {
    // Unregister services
    UnregisterProvidedService();
    UnregisterAcceptedService();
    
    // Stop the handler Thread
    iServerActive = false;
    SetActive(false);
    dPrint(4,"Succesfully Close():ed PositionServer instance %p", this);
  }
  
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CPositionServer::WaitCommand(CPositionData &aData, 
                                              const unsigned int aTimeout_ms)
{
  EMaCIError e,result = KMaCIError;
  gimi::GIMIMessage gmsg;
  const unsigned int tb = ownTime_get_ms();
  int timeleft = aTimeout_ms; // timeleft
  
  // Run as long as the thread should be active & timeout not hit
  while(iServerActive && timeleft >= 0) {
    
    // If more than 0ms is available, do waiting.
    e = ReceiveInterfaceMessage(gmsg, timeleft);
    if (e == KMaCIOK) {
      gim::binbag::CBinBag bb;
      
      // Decode BinBag
      bool r = bb.Decode(gmsg.getData(), gmsg.getDatalength());
      
      if (r) {
        // if decoding as BinBag was OK; Decode as PositionData
        CPositionData pdata;
        r = pdata.DecodeFrom(&bb);
        if (r && pdata.GetCommand()) {
          // If decoding as PositionData was OK, and GetCommand() returned
          // NON-null parameters. Copy data to caller and set result to true,
          aData.Copy(pdata);
          result = KMaCIOK;
          break;
        }
      }
    }
    
    timeleft = aTimeout_ms - ownTime_get_ms_since(tb);
  }

  // Check for timeout and set status approriately
  if (timeleft < 0 && result == KMaCIError) result = KMaCITimeout;

  return result;
}
//*****************************************************************************

bool CPositionServer::SendPositionEvent(const CPositionData &aData)
{
  bool result = false;

  assert(iGIMIPtr != NULL);
  //aData.Print(1);
  const TPose2D *p = aData.GetPose2D();
  const TPose3D *p3 = aData.GetPose3D();
  if (p || p3) {
    EMaCIError e = 
      SendInterfaceSubscriberMessage(aData.GetBinBagContainerPtr()->GetBuffer(),
                                     aData.GetBinBagContainerPtr()->GetBufferSize());
    result = (e == KMaCIOK);

  } else {
    dPrint(4,"No Pose2D element in container, will not send!");

  }

  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
